通信流程图

nodelet内部 wbc，mrt和状态估计数据流

mpc输入端

反馈端

Kuavo Dynamic Control topic logical

mpc_controller output and nodelet input

Kuavo Dynamic Control startup logical structure

各参数配置文件

跳过mpc的nodelet controller 输入

ocs2待看 /mpc ocs2控制器

灵巧手相关topic

Kuavo Dynamic Control file structure

mpc ocs2控制器

measuredRbdState_topic

遥控轨迹输入

Rviz 显示

core

命令行终端步态输入

/humanoid/desiredBaseTrajecory
rviz display

/humanoid/currentState

/humanoid_sqp_mpc

/nodelet_manager

/humanoid_hand_control

/humanoid_quest_control_with_arm

/state_estimate/Contact_Detection/mode

/humanoid_controller/swing_leg/pos_measured
Type: std_msgs/Float64MultiArray
作用待验证

/humanoid_sqp_mpc

/humanoid_joy_control_auto_gait_with_vel

/humanoid/mpc/terrainHeght
Type: std_msgs/Float64

/hand_wrench/left_hand
左手力矩数据

/humanoid/optimizedStateTrajectory

/humanoid/planedFootPositions
rviz display

/hand_wrench/right_hand
右手力矩数据

/humanoid_VR_hand_control

表示使用较多的核心节点

/humanoid_ee_State
Type: kuavo_msgs/endEffectorData

/humanoid_hand_control

humanoid_controller

表示普通节点

表示话题发布

humanoid_common

/humanoid_single_step_control
type：kuavo_msgs/singleStepControl
footPoseTargetTrajectories
单步控制，通过给出时间序列和对应的躯干位姿，可以控制机器人的单步行走
时间序列和躯干位姿序列长度必须一致，时间序列需要不断递增
每次服务请求的躯干位姿都是基于局部坐标系，但是一次服务请求中的躯干位姿序列需要以第一个位姿为基准不断变化

/gesture/list
列出所有预设的手势

/gesture/execute
获取当前控制模式，返回 control_mode

/gesture/execute_state
获取当前控制模式，返回 control_mode

/humanoid_joy_control_auto_gait_with_vel

/nodelet_manager

/humanoid_change_arm_ctrl_mode
受srv控制，需要是external_control_mode
服务端：/humanoid_sqp_mpc

/kuavo_arm_target_poses
type：kuavo_msgs/armTargetPoses
控制机器人手臂在指定时间内到达目标位置

/humanoid_VR_hand_control

/humanoid_Arm_time_target_control

/humanoid_get_arm_ctrl_mode
获取当前控制模式，返回 control_mode

/humanoid_mpc_mode_scale
type: std_msgs/Float32
步态的缩放比例,用于控制步频topic

/humanoid_mpc_stop_step_num
type: std_msgs/Int32
停止步数，从当前统计的步数开始，机器人会在后续第N步自动停下

/cmd_vel
type: geometry_msgs/Twist
控制指令，6dof 速度指令，机器人的target指令的速度形式，包含xy方向速度、高度z和yaw方向速度，但 roll、pitch 方向不控制

/humanoid_mpc_target
type：ocs2_msgs/mpc_target_trajectories
发送给mpc的期望状态topic

/humanoid_mpc_target_arm
type：ocs2_msgs/mpc_target_trajectories
发送给mpc的期望状态topic

/humanoid_mpc_target_pose
type：ocs2_msgs/mpc_target_trajectories
发送给mpc的期望躯干状态6dof 位姿规划指令
注意位姿指令优先级比 cmd_vel 指令高，不要同时发送两种指令

/humanoid_hand_control

/humanoid_quest_control_with_arm

/joystick_node
这是一个伪节点，可以是遥控也可以是terminal脚本

/humanoid_mpc_foot_pose_target_trajectories
type: kuavo_msgs/footPoseTargetTrajectories
用于单步控制，指定脚的步态位姿，以及对应的躯干姿态。

/humanoid_mpc_mode_schedule
type：ocs2_msgs/mode_schedule
步态序列切换topic

/cmd_pose
type: geometry_msgs/Twist
位置控制指令, 可用与控制机器人从当前位置到达目标 pose

/humanoid_sqp_mpc

关键算法和技术指标：

用到的算法
NMPC+WBC
NMPC：SQP+ODE
状态估计：线性卡尔曼滤波，不变扩展卡尔曼滤波(InEKF)
接触检测：基于卡尔曼滤波和广义动量估计的足端接触足底力；基于关节冲量的接触检测。
涉及到的传感器：
IMU，关节编码器
各个控制模块频率：
mpc 50hz
sensor raw frequency 500hz
wbc 500hz
用到的约束：
MPC：
手臂末端零力约束
摩擦锥约束
躯干picth角约束，包含速度和位置约束
支撑相0速度约束，软约束
摆动相x-y方向跟踪约束，软约束
selfCollision约束，自碰撞约束
WBC：
浮动基动力学约束：硬约束
关节扭矩约束：硬约束
摩擦锥约束：软约束

/humanoid_auto_gait
srv：是否自动切换gait

/joy
Type: sensor_msgs/Joy

/humanoid_joy_control_auto_gait_with_vel

收集启动信息脚本分析: 记录和管理ROS启动环境信息的工具，对于异常排查和系统监控(sh脚本)

脚本主要功能

收集启动环境信息：获取Git版本控制信息、时间戳、机器人序列号等
创建唯一的信息存储目录：基于父进程ID创建唯一的目录
管理历史记录：限制存储的启动记录和coredump数量
记录崩溃环境：保存可能发生崩溃的环境信息

工作流程

信息收集阶段
- 获取Git相关信息及仓库同步状态
- 识别是否为主仓库代码或分支仓库
- 尝试读取机器人序列号
信息存储阶段
- 创建信息存储目录
- 将收集到的所有信息写入info.txt文件
清理维护阶段
- 检查历史启动记录数量
- 如果超过MAX_DIRS(50)，删除最旧的记录
- 同样管理coredump目录，防止磁盘占用过多

robot_version_management 机器人版本管理以及rviz可视化配置(launch)

确定并设置机器人版本参数
加载对应版本的机器人URDF模型
配置与版本相关的控制参数文件
可选地启动可视化工具

配置mpc控制器可视化参数

json：

kuavo.json：

model_path：用于计算初始化的下肢各关节逆解的模型路径
model_with_arm_path：参与踝关节并联杆解算的模型路径
end_frames_name：参与初始化逆解的躯干和下肢末端在 URDF 中的 Link 名称
ankle_solver_type：机器人使用的踝关节换算方法类型 (0为4代，1为4代pro，2为5代)
torsoP：初始化时，期望的躯干pitch角度，单位：度
com_z：初始化时，期望的质心z方向高度，单位：m
StepWith：初始化时，脚掌中心距离躯干中心的y方向距离，单位：m
torsoY：初始化时，期望的躯干yaw角度，单位：度
imu_invert：IMU 的装配是否上下颠倒
isParallelArm：是否使用并联杆手臂
only_half_up_body：是否只启用上半身（头部+手臂）的控制
MOTORS_TYPE：各关节的电机类型，需与实际电机数量匹配
EndEffectorType：双臂的末端执行器类型
min_joint_position_limits & max_joint_position_limits：各关节的上下限位角度，单位：度
joint_velocity_limits：各关节的转速限幅，单位：度/秒
end_frames_name_ik：手臂 IK 关联的模型中躯干，手掌，肘关节的连杆名称
shoulder_frame_names：手臂 IK 关联的第一个关节连杆名称
eef_visual_stl_files：仿真中可视化末端的模型文件名称
upper_arm_length & lower_arm_length：VR 操作时，机器人的大臂和小臂长度
motor_velocities_factor：头部电机速度期望的缩放因子

json配置文件说明

Info

gait.info：用于调整机器人步态的切换和对应的落足时序；

list中有可选取步态：{[0] stance, [1] trot, [2] jump, [3] sl ; single leg stance, [4] walk, [5] walk2, [6] trot2}

modeSequence：接触模式，用于描述机器人双脚与地面的接触情况；
switchingTimes：用于描述不同接触模式的持续时间，与接触模式一一对应；
modeSequence 是两个英文字母组成的字符串，单个英文描述单腿的接触情况，先左腿后右腿。对于单条腿，接触模式有以下几种:
S：stance，表示脚掌完全接触地面；
F：floating，表示脚掌完全离开地面；humanoid_mpc_stop_step_num
T：Toe，表示仅脚尖接触地面；
H：Heel，表示仅脚跟接触地面；
example:

walk
{
 modeSequence
  {
    [0]     SS
    [1]     ST
    [2]     SF
    [3]     SH
    [4]     SS
    [5]     TS
    [6]     FS
    [7]     HS

  }
  switchingTimes
  {
    [0]     0.05
    [1]     0.07
    [2]     0.35
    [3]     0.04
    [4]     0.05
    [5]     0.07
    [6]     0.35
    [7]     0.04
  }
}

back_walk
{
 modeSequence
  {
    [0]     SS
    [1]     SS
    [2]     SF
    [3]     ST
    [4]     SS
    [5]     SS
    [6]     FS
    [7]     TS

  }
  switchingTimes
  {
    [0]     0.05
    [1]     0.07
    [2]     0.35
    [3]     0.04
    [4]     0.05
    [5]     0.07
    [6]     0.35
    [7]     0.04
  }
}

reference.info：用于调整遥控器的限幅，和各关节的控制模式和上层扭矩的kpkd；

规划层的限幅（遥控器指令限幅（只关联速度和角速度），VR操作时手臂摆动的速度限幅）；
1. VR目标移动速度，旋转角速度；命令移动速度旋转角速度限幅；VR使用时torso胯部即质心位置的速度和角速度限幅。
2. 质心高度，是否开启手臂模式。
关节层的电机模式以及电机限幅和对应的kp，kd参数；
1. jointStateLimit: 各关节电机电机位置限幅频率
2. defaultCotrolMode: 各关节电机控制模式；
3. jointkp: 站立模式下，各关节(除了头部) CST 模式下的扭矩指令，EC主站中位置环对应的kp系数 (2kHz);
4. jointkd: 站立模式下，各关节(除了头部) CST 模式下的扭矩指令，EC主站中速度环对应的kd系数 (2kHz);
5. jointkp_walking: 机器人运动时，对应关节 CST 模式下，EC主站中位置环对应的kp系数 (2kHz);
6. jointkd_walking: 机器人运动时，对应关节 CST 模式下，EC主站中位置环对应的kp系数 (2kHz);
7. headkp: 上层控制器下发头部扭矩时，执行的位置环对应的kp系数 (500Hz);
8. headkd: 上层控制器下发头部扭矩时，执行的速度环对应的kd系数 (500Hz);
9. IMU和各关节以及速度的滤波频率；
10. mrt_joint_vel_filter_cutoff_freq：Motion reference trajectory的低通滤波截止频率。
11. 初始化模式序列合各个指令参数（站立时固定高度，速度角速度为0,关节位置固定）

dynamic_qr.info：MPC 根据步态，切换到对应的QR矩阵；

若启用 dynamic_qr，则要保证矩阵维度对应于变量的维度，否则求解器报错；
目前仅 DDP 支持，若使用 SQP 求解器，则 dynamic_qr 不会生效。
支持三种Q矩阵的切换：
Q_base_stance：stance 步态时生效；
Q_base_walk：walk 和 trot 步态生效；
Q_base_jump：jump 步态生效；

task.info：MPC 和 WBC 各任务和约束，主要的修改文件；

MPC 的模型设置；
运动时，MPC 全身摆动轨迹的配置；
MPC 求解器设置；
MPC 使用的权重矩阵 Q，P，R；
MPC 各种软约束和硬约束的参数；
WBC 的扭矩约束和摩擦锥约束；
WBC 加速度任务的增益系数；
WBC 各加速度任务的权重；
卡尔曼滤波器的参数设置；
接触力估计的参数设置；
代码运行时，重要频率的监测数据；

参数说明：

几乎不修改：

centroidalModelType：0: FullCentroidalDynamics, 1: Single Rigid Body Dynamics；
humanoid_interface：包含是否添加加载参数，解析梯度动力学模型计算和解析末端足底力运动学计算。
SQP多重射击算法，求解器参数：
- nThreads：sqp 求解时，参与并行计算的最大CPU数量9
- dt：用户定义的动力学离散化时间0.015
- sqpIteration：sqp 求解的迭代次数，次数多则增加求解时间
- deltaTol：x(t)和u(t)的均方根误差小于该值，终止迭代
- g_max & g_min：g 表示违反约束的值函数的范数, 分别表示最大和最小值
- inequalityConstraintMu & inequalityConstraintDelta：表示不等式约束违反的宽松程度
- projectStateInputEqualityConstraints：是否使用投影方法求解硬约束
- printSolverStatistics & printSolverStatus & printLinesearch：求解过程的相关信息打印
- useFeedbackPolicy：是否启用 RiccatiFeedback
- integratorType：动力学离散化方法
- threadPriority：MPC 求解时，所使用线程的优先级
DDP反向迭代差分算法，求解器参数：
IPM多重射击算法，求解器参数：
rollout：ODE求解器参数：
- AbsTolODE：ODE 求解器的绝对容忍误差
- RelTolODE：ODE 求解器的相对容忍误差
- timeStep：逐步推进动力学模拟的时间步长
- integratorType：推进动力学模拟使用的积分器类型ODE45
- maxNumStepsPerSecond：ODE 求解器每秒最大的积分点数量
- checkNumericalStability：布尔变量，是否需要检查 rollout 过程的数值稳定性
mpc固定参数：
- timeHorizon：MPC 预测时域，越长则求解耗时越长
- solutionTimeWindow：希望保留的优化输出的时间序列长度，负数表示检索整个预测时域的优化输出
- recordSolverData：布尔量，表示是否要回放MPC求解结果
- coldStart：每次运算MPC是否重置，通常不重置，求解速度更快
- debugPrint：在终端中实时打印MPC调用的时间信息
- mpcDesiredFrequency & mrtDesiredFrequency：mpc和mrt模块的期望输出频率
MonitorParams：监控参数报警：
- totalNum：监测器的数据队列长度
- recentNum：监测器对异常给予警报的新数据数量
- warningThreshold & errorThreshold：警告/报错倍数，若最新数据大于队列数据的均值加"方差*倍数"，则打印警告/错误信息
- mpcFrequencyBiasPercent & mpcFrequencyBiasPercent：频率阈值百分比，mpc/wbc 需保持在期望频率的上下百分比区间内，否则打印警报；若队列所有数据均在阈值区间外，则打印报错
- printWarning：布尔量，是否将警告信息在终端中打印
- contactcst_st：时间长度，单位:s。膝关节 kpkd 在修改步态前的设置时间之后，会切换为 joint_kp_walking 中的设置
- contactcst_et：时间长度，单位:s。膝关节 kpkd 在修改步态后的设置时间之内，会切换为 joint_kp_walking 中的设置

MPC相关参数设置：

model_settings：
- positionErrorGain：
- positionErrorGain_xy：
- velocityErrorGain_xy：下肢摆动足跟踪约束的位置和速度误差增益
- phaseTransitionStanceTime：切换为站立的延迟时间
- verboseCppAd：打印 CppAD 的装载情况
- recompileLibrariesCppAd：若路径已存在文件，是否重新编译CppAD，默认为false
- modelFolderCppAd：CppAD 文件的缓存路径，需与版本号保持一致
- modelDof：全身关节总的自由度
- mpcArmsDof：参与MPC计算的手臂自由度
- mpcLegsDof：参与MPC计算的下肢自由度
- jointNames：共可参与MPC计算的关节名称，与 modelDof 关联
- jointNamesReal：实物机器人可控制的关节名称，与实物电机数量关联
- contactNames6DoF：参与MPC计算的六维接触点，通常是上肢，末端两个点
- contactNames3DoF：下肢八个支撑相点
- eeFrame：MPC中与末端六维跟踪约束相关的坐标系名称
swing_trajectory_config：摆动相轨迹配置
- liftOffVelocity：机器人抬腿的末端z方向初速度
- touchDownVelocity：机器人落足的末端z方向速度
- swingHeight：机器人抬腿的最高高度
- swingTimeScale：如果摆动相总时间小于该值，则抬腿高度会降低
- toeSwingHeight：拟人步态中，脚跟着地时，脚尖的z方向高度
- heelSwingHeight：拟人步态中，脚尖着地时，脚跟的z方向高度
- deadBandVelocity：拟人步态中，躯干x方向速度小于该值，则退化为trot
- heelToeMaxHeightVelocity：拟人步态中，躯干x方向速度小于该值，则将拟人步态的脚跟脚尖抬升高度进行缩小
- swing_shoulder_center：摆臂时肩膀pitch电机的转动中心(单位:rad)
- swing_shoulder_scale：摆臂时肩膀pitch电机的摆动范围(单位:rad)
- swing_elbow_scale：摆臂时肘关节电机的摆动范围(单位:rad)
Q&P：Q为过程状态权重，P为终端状态权重(默认不启用)，内部 scaling 为缩放因子，将矩阵数据统一缩放，权重的顺序为：归一化质心动量，基坐标位姿，关节位置
R：R为控制权重矩阵，scaling为缩放因子，权重的顺序为：三维接触点的接触力，六维接触点的接触力，三维接触点相对躯干的摆动速度，手臂各关节速度
enableZeroSixDofForceConstraint：布尔量，表示是否启用手臂末端零力约束
frictionConeSoftConstraint：
- frictionCoefficient：摩擦锥约束中设置的摩擦系数
- mu：缩放因子，越大则约束惩罚程度越大
- delta：松弛屏障阈值，越小则越接近对数屏障
basePitchLimits：表示对躯干 pitch 的 state-only 约束，限制 mrt 中 pitch 的角度和角速度的输出范围
zeroVelocityConstraint：支撑相0速度约束，布尔量，是否启用软约束，约束支撑相零速度，默认需要启用，硬约束求解难度较大
softSwingTrajConstraint：摆动足的x-y方向跟踪软约束的权重
selfCollision：自碰撞约束，可写入碰撞对，用于约束URDF中对应名称的连杆不可碰撞

WBC相关参数设置

torqueLimitsTask：全身各电机的扭矩约束，左右需对称，仅设置一组
Wbcrdd_K：质心位置跟踪加速度任务的 kp/kd， stance 在双支撑阶段生效, walk 在之外的阶段生效
frictionConeTask：frictionCoefficient，WBC 中摩擦锥约束的摩擦系数
swingLegTask：摆动足跟踪加速度任务，kp/kd 表示z方向跟踪增益, kp/kd_xy 表示x-y方向跟踪增益， real 在实物中生效，sim 在仿真中生效
baseAngularTask：躯干角度跟踪加速度任务，kp/kd 的顺序为 roll/pitch/yaw
armAccelTask：手臂关节跟踪加速度任务，对应的各关节kp/kd
weight：stance 在双支撑阶段生效，walk 在之外的阶段生效，若干参数表示各自任务对应的权重

状态估计相关的参数设置

kalmanFilter：

footRadius：脚掌 z 方向的偏置，单位: m
imuProcessNoisePosition：imu 估计位置的过程噪声协方差
imuProcessNoiseVelocity：imu 估计速度的过程噪声协方差
imuProcessNoiseZPosition：z方向 imu 估计位置的过程噪声
imuProcessNoiseZVelocity：z方向 imu 估计速度的过程噪声
footProcessNoisePosition：引入足端位置的过程噪声协方差
footSensorNoisePosition：引入足端位置的测量噪声协方差
footSensorNoiseVelocity：引入足端速度的测量噪声协方差
footHeightSensorNoise：足端高度的测量噪声协方差
contactForceEsimation：
cutoffFrequency：测量广义动量的截止频率
contactThreshold：判断足端接触状态的z方向阈值
detectCutoffFrequency：基于关节冲量的接触检测，对应滤波器的截止频率
maxEnergyThreshold：基于关节冲量的接触检测，对应的一些阈值设置
minEnergyThreshold：
maxEnergyThreshold2：
minEnergyThreshold2：
timetreshold：两次接触状态判断，之间的时间间隔阈值

info配置文件说明

ocs2_topics.jpg

targetmanager.png

启动humanoidsqpmpc节点(node)，允许在节点前启动脚本 start_node.sh

/control_robot_hand_position
type: kuavo_msgs/robotHandPosition
只有在kuavo.json中配置EndEffectorType为qiangnao或qiangnao_touch时才会发布该话题.
话题描述: 用于控制机器人双手(手指)的运动，通过发布手指目标关节位置来实现手部的精确控制。

/dexhand/command
type: kuavo_msgs/dexhandCommand
只有在kuavo.json中配置EndEffectorType为qiangnao_touch时才会发布该话题.
话题描述: 用于控制机器人双手(手指)的运动，通过发布手指目标关节位置来实现手部的精确控制。

<?xml version="1.0" ?>

<launch>

<arg name="rviz" default="false" />

<arg name="description_name" default="humanoid_description"/>

<arg name="robot_version" default="$(optenv ROBOT_VERSION 40)"/>   默认从环境变量ROBOT_VERSION 读取，如果不存在设为40

<param name="robot_version" value="$(arg robot_version)"/>

<!-- <arg name="urdfFile" default="$(find biped_s3)/urdf/biped_s3_with_v3_arm.urdf"/> -->

<arg name="urdfFile" default="$(find kuavo_assets)/models/biped_s$(arg robot_version)/urdf/biped_s$(arg robot_version).urdf"/>
`urdfFile`: 机器人的URDF模型文件路径，基于版本号动态生成
`description_name`: ROS参数服务器中存储URDF的参数名

<!-- params -->
laucn中进行参数设置直接引入工程代码
- `taskFile`: MPC 和 WBC 各任务和约束，主要的修改文件；
- `referenceFile`: 用于调整遥控器的限幅，和各关节的控制模式和上层扭矩的kpkd；
- `dynamicQrFile`: 动态权重配置，MPC 根据步态，切换到对应的QR矩阵；
- `gaitFile`: 步态命令配置，定义不同行走模式，以及步态相位顺序和对应时间
- `kuavoConfigFile`: 机器人JSON配置文件，包含关键硬件参数可视化参数
- `rviz`: 是否启动RViz可视化工具，默认为false
- `rvizconfig`: RViz配置文件路径
<param name="urdfFile" value="$(arg urdfFile)" />

<param name="taskFile" value="$(find humanoid_controllers)/config/kuavo_v$(arg robot_version)/mpc/task.info"/>

<param name="referenceFile" value="$(find humanoid_controllers)/config/kuavo_v$(arg robot_version)/command/reference.info"/>

<param name="dynamicQrFile" value="$(find humanoid_controllers)/config/kuavo_v$(arg robot_version)/mpc/dynamic_qr.info"/>

<param name="gaitCommandFile" value="$(find humanoid_controllers)/config/kuavo_v$(arg robot_version)/command/gait.info"/>

<arg name="kuavoConfigFile" default="$(find kuavo_assets)/config/kuavo_v$(arg robot_version)/kuavo.json"/>

<param name="kuavo_configuration" textfile="$(arg kuavoConfigFile)"/>

<param name="modelPath" value="$(find kuavo_assets)/models" />

<!-- rviz -->

<!-- <group if="$(arg rviz)"> -->

<param name="$(arg description_name)" textfile="$(arg urdfFile)"/>

<arg name="rvizconfig" default="$(find humanoid_interface_ros)/rviz/humanoid.rviz" />

<node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(arg rvizconfig)" output="screen" />

<!-- </group> -->

</launch>

robot_version_manager.launch

启动mujoco仿真 nodelet_with_arm launch文件
启动rosbag_nodelet节点
启动ros_joytick节点
启动系统信息发布节点
启动kuavo状态监控节点

启动ros nodelet manager 为镜像配置参数为

10 worker threads to handle concurrent processing tasks

启动加载一个 actual controller implementation (named nodelet_controller (controller 节点)) 到nodelet manager中

启动humanoid_gait_command节点(node): 步态命令生成器，控制机器人行走方式
启动humanoid_hand_control节点(node): 机器人手部控制
启动humanoid_VR_hand_control节点(node): VR手部控制
启动humanoid_quest_control_with_arm节点(node): 使用Quest设备控制机器人手臂
启动humanoid_Arm_time_target_control节点(node): 机器人手臂时间轨迹控制
启动humanoid_gait_switch_by_name节点(node): 用于切换不同预定义步态

允许在节点前启动脚本 start_node.sh

Humanoid-control

humanoid_interface_ros

qpoases_catkin

/dexhand/left/command
type: kuavo_msgs/dexhandCommand
只有在kuavo.json中配置EndEffectorType为qiangnao_touch时才会发布该话题.
话题描述: 用于控制机器人双手(手指)的运动，通过发布手指目标关节位置来实现手部的精确控制。

/leju_claw_command
type: kuavo_msgs/lejuCalwCommand
只有在kuavo.json中配置EndEffectorType为lejuclaw时才会发布该话题.
话题描述: 话题描述: 该话题用于控制二指夹爪
注意/警告：如果当前夹爪处于运动状态，那么发送新的请求就会被丢弃处理，不会执行，望知悉。

/dexhand/right/command
type: kuavo_msgs/dexhandCommand
只有在kuavo.json中配置EndEffectorType为qiangnao_touch时才会发布该话题.
话题描述: 用于控制机器人双手(手指)的运动，通过发布手指目标关节位置来实现手部的精确控制。

dexhand/touch_state
type: kuavo_msgs/dexhandTouchState
话题描述: 发布触觉灵巧手的触觉状态数据.

/control_robot_leju_claw
type: kuavo_msgs/controlLejuClaw
只有在kuavo.json中配置EndEffectorType为lejuclaw时才会发布该话题.
该服务用于控制二指夹爪

/leju_claw_state
type: kuavo_msgs/lejuClawState
话题描述: 发布二指夹抓的状态, 位置, 速度, 力距等信息

表示服务信息

表示废弃话题

/humanoid_gait_command

/humanoid_gait_switch_by_name

/humanoid_controller/wbc_planned_body_acc/linear
type：std_msgs/Float64MultiArray
wbc优化后的躯干线性加速度,, 单位(rad/s)

Node：

/humanoid_Arm_time_target_control
/humanoid_VR_hand_control
/humanoid_gait_command
/humanoid_gait_switch_by_name
/humanoid_hand_control
/humanoid_joy_control_auto_gait_with_vel
/humanoid_quest_control_with_arm
/humanoid_sqp_mpc
/joystickSimulator
/kuavo_monitor_node
/nodelet_controller
/nodelet_manager
/nodelet_mujoco
/nodelet_rosbag
/rosout
/rviz
/system_info_publisher

ROS_simulate_mujoco_startup_Sequence

一些重要的参数指标
wbc frequency 500hz
sensor frequency 500hz
arm_move_spd 3.0

NMPC：顺序二次规划(SQP)模型预测控制(MPC)节点
负责生成最优运动轨迹规划

#include <ros/init.h>

#include <ros/package.h>

  

#include <ocs2_sqp/SqpMpc.h>

#include <humanoid_interface/HumanoidInterface.h>

#include <ocs2_ros_interfaces/mpc/MPC_ROS_Interface.h>

#include <ocs2_ros_interfaces/synchronized_module/RosReferenceManager.h>

#include <ocs2_ros_interfaces/synchronized_module/SolverObserverRosCallbacks.h>

  

#include "humanoid_interface_ros/gait/GaitReceiver.h"

#include "ocs2_core/thread_support/SetThreadPriority.h"

using namespace ocs2;

using namespace humanoid;

  

int main(int argc, char **argv)

{
const std::string robotName = "humanoid";

  

// Initialize ros node

::ros::init(argc, argv, robotName + "_mpc_sqp");

::ros::NodeHandle nodeHandle;

// Get node parameters

bool multiplot = false;

std::string taskFile, urdfFile, referenceFile,gaitCommandFile;

nodeHandle.getParam("/multiplot", multiplot);

nodeHandle.getParam("/taskFile", taskFile);

nodeHandle.getParam("/referenceFile", referenceFile);

nodeHandle.getParam("/urdfFile", urdfFile);

nodeHandle.getParam("/gaitCommandFile", gaitCommandFile);

  

int version_num;

if (nodeHandle.hasParam("/robot_version"))

{

nodeHandle.getParam("/robot_version", version_num);

}

// Robot interface

HumanoidInterface interface(taskFile, urdfFile, referenceFile, gaitCommandFile, version_num);

interface.setupCPUconfig();// 配置内核隔离和线程优先级

// Gait receiver

auto gaitReceiverPtr =

std::make_shared<GaitReceiver>(nodeHandle, interface.getSwitchedModelReferenceManagerPtr(), robotName);

  

// ROS ReferenceManager

auto rosReferenceManagerPtr = std::make_shared<RosReferenceManager>(robotName, interface.getReferenceManagerPtr());

rosReferenceManagerPtr->subscribe(nodeHandle);

  

// MPC

SqpMpc mpc(interface.mpcSettings(), interface.sqpSettings(), interface.getOptimalControlProblem(),

interface.getInitializer());

mpc.getSolverPtr()->setReferenceManager(rosReferenceManagerPtr);

mpc.getSolverPtr()->addSynchronizedModule(gaitReceiverPtr);

  

// Launch MPC ROS node

MPC_ROS_Interface mpcNode(mpc, robotName);

mpcNode.launchNodes(nodeHandle);

  

std::cout << "!!!!!!!!!!!!!!!!!!!end!!!!!!!!!!!!!!" << std::endl;

// Successful exit

return 0;

}

humanoid_sqp_mpc node

<launch>

<arg name="legged_robot_scene" default="$(find kuavo_assets)/models/biped_s$(optenv ROBOT_VERSION 40)/xml/scene.xml"/>

<param name="legged_robot_scene_param" value="$(arg legged_robot_scene)" />

<node pkg="nodelet" type="nodelet" name="nodelet_mujoco" args="load MujocoNodelet nodelet_manager" respawn="false" output="screen" />

</launch>

nodelet_with_arm.launch

/humanoid_VR_hand_control

/nodelet_manager

/robot_head_motion_data
type：kuavo_msgs/robotHeadMotionData
用于控制机器人头部的运动，通过发布目标关节角度来实现头部控制,位控。

/类似于手臂重映射，编码器命令直接下发

/kuavo_arm_traj
type：sensor_msgs/JointState
用于控制机器人手臂运动，通过发布手臂目标关节位置来实现手臂的精确控制，目前位控.

/nodelet_manager

/humanoid_joy_control_auto_gait_with_vel

/humanoid_change_arm_ctrl_mode
受srv控制，需要是外部控制？

/humanoidcontroller/swing_leg/vel_measured*
type：std_msgs/Float64MultiArray
发布接触点当前的测量速度, 单位(m/s)

/humanoid_controller/wbc_planned_body_acc/angular
type：std_msgs/Float64MultiArray
wbc优化后的躯干角加速度, 单位(rad/s)

hardware_interface

Demo/graspBox

/humanoid_joy_control_auto_gait_with_vel

/humanoid_foot_contact_point
type：std_msgs/Float32MultiArray

/humanoid_sqp_mpc

/humanoid_mpc_arm_commanded
type：ocs2_msgs/mpc_target_trajectories
发布当前使用的手臂目标轨迹

/humanoid_mpc_gait_time_name
type： kuavo_msgs/gaitTimeName
步态的时间和名字topic

dexhand/state
type: sensor_msgs/JointState
话题描述: 发布灵巧手的状态数据.

/enable_wbc_arm_trajectory_control

/humanoidcontroller/swing_leg/pos_measured*
type：std_msgs/Float64MultiArray
发布接触点当前的测量位置, 单位(m) 根据状态估计发布的质心位置，结合前向运动学发布

/humanoid_joy_control_auto_gait_with_vel

/humanoid_mpc_policy
Type: ocs2_msgs/mpc_flattened_controller
mpc计算的结果

/nodelet_manager

/humanoid_sqp_mpc

/humanoid_controller/com/com_lf_diff
type：std_msgs/Float64MultiArray
左脚质心位置 xyz, 单位(m)

/humanoid_controller/com/com_lf_diff_des
type：std_msgs/Float64MultiArray
左脚质心期望位置 xyz, 单位(m)

/humanoid_controller/com/com_rf_diff
type：std_msgs/Float64MultiArray
右脚质心位置 xyz, 单位(m)

/humanoid_controller/com/rd_base
type：std_msgs/Float64MultiArray
质心期望相对base坐标系的位置

/humanoid_controller/swing_leg/vel_desired
type：std_msgs/Float64MultiArray
数组长度24, 即左右接触点(0~7) xyz 的期望速度
发布接触点的期望速度, 单位(m/s)

/humanoid_controller/swing_leg/vel_measured
type：std_msgs/Float64MultiArray
发布接触点当前的实际测量速度速度, 单位(m/s)

初始化控制器，- Initializes the controller with hardware interface, node handle, and nodelet flag

/humanoid_controller/com/rd_des
type：std_msgs/Float64MultiArray
质心期望速度

/humanoid_controller/com/r_des
type：std_msgs/Float64MultiArray
质心期望位置

hardware_interface

gazebo

/humanoid_controller/com/r
type：std_msgs/Float64MultiArray
质心位置

/humanoid_controller/feet_target_policys

/humanoid_controller/com/rd
type：std_msgs/Float64MultiArray
质心速度

/enable_mpc_flag
控制 MPC 启用/禁用

/enable_wbc_flag
控制 WBC 启用/禁用

/humanoid_controller/swing_leg/pos_measured
type：std_msgs/Float64MultiArray
发布接触点当前的测量位置, 单位(m) 根据状态估计发布的质心位置，结合前向运动学发布

/hand_wrench_cmd
手部力矩命令

加载控制器稳定数据，传感器数据流_preUpdate

wbc内部变量 logger发布

mujoco_nodelet

/sensors_data_raw
type：kuavo_msgs/sensorsData
实物机器人, 仿真器发布的传感器原始数据

/joint_cmd
type：kuavo_msgs/jointCmd
关节命令下发

/bot_stand_up_complete

控制器主循环

使能控制器_starting

1.设置机器人参数配置，包含版本号，关节数，初始高度，传感器种类等；
2.根据info文件设置机器人的配置参数；
3.初始化共享内存
4.初始化gait manager，默认启动步态stance
5.初始化ddp_mpc
6.初始化mrt_mpc
7.初始化pinocchio可视化
8.初始化硬件接口等
9.初始化状态估计等

时间戳和性能监控：记录开始时间。
更新传感器数据：如果使用共享内存，调用 updateSensorDataFromShm 获取最新数据。
重置 MPC：如果需要（resetmpc 为真），重置 MPC 状态。
状态估计更新：调用 updateStateEstimation 更新机器人状态，包括位置、速度和接触力。
MPC 执行：根据配置（外部或内部 MPC），更新当前观察并运行规划，获取优化状态和输入。
处理控制模式：根据标志（如 wbconly）选择使用 MPC 结果或默认值。
调整简化模型：如果使用简化模型，映射 MPC 输出到真实机器人。
全身控制（WBC）：调用 wbc_->update 计算关节控制命令。
安全检查：使用 safetyChecker_->check 确保安全。
生成并发布控制命令：计算关节位置、速度和力矩，发布到 /joint_cmd。
可视化和日志：更新可视化工具和发布性能指标。
清理和准备下一周期：记录结束时间，更新状态。

选择控制器类型

nodelet_Controller

state_estimation callback function

/sensors_data_raw

controller_nodelet

/start_mpc

/humanoid_controller/optimizedState_mrt/com/angular_vel_xyz
type：std_msgs/Float64MultiArray
从mpc(mrt)取得的质心线速度, 顺序为xyz, 单位(m/s)

/humanoid_controller/swing_leg/acc_desired
type：std_msgs/Float64
发布接触点期望加速度, 单位( m/s2 )

/humanoid_controller/optimizedState_mrt/joint_pos
type：std_msgs/Float64MultiArray
从mpc(mrt)取得的关节位置, 单位(radian)

/stateestimate/end_effector/contact_point*/feet_height

估计的第x个接触点的"足端高度"

/state_estimate/base/angular_vel_zyx

估计的躯干角速度,顺序为zyx

/state_estimate/base/linear_vel

估计的躯干线速度,顺序为xyz

/state_estimate/base/angular_zyx

估计的躯干线速度,顺序为xyz（ypr）

/humanoidcontroller/swing_leg/acc_desired*
type：std_msgs/Float64MultiArray
发布特定序号的接触点期望加速度, 单位(m/s2)

humanoid_controller/optimizedState_mrt/base/pos_xyz
type：std_msgs/Float64MultiArray
从mpc(mrt)取得的躯干位置, 单位(m)

/humanoid_controller/optimizedState_mrt/base/angular_zyx
type：std_msgs/Float64MultiArray
从mpc(mrt)取得的躯干角速度, 顺序为zyx, 单位(radian/s)

/humanoid_controller/optimizedState_mrt/com/angular_zyx
type：std_msgs/Float64MultiArray
从mpc(mrt)取得的质心角速度, 顺序为zyx, 单位(radian/s)

/stateestimate/end_effector/contact_point*/pos

第x个接触点的位置

/state_estimate/est_arm_contact_force

估计的手臂接触力

/stateestimate/end_effector/contact_point*/vel

第x个接触点的速度

/state_estimate/Contact_Detection/contactForce

估计的足底力

/state_estimate/Contact_Detection/contactProbability

估计的足底接触概率

/humanoidcontroller/optimizedInput_mrt/force*
type：std_msgs/Float64MultiArray
从mpc(mrt)取得的第x个接触点的接触力, 单位(N)

/state_estimate/mode

估计的步态mode

/state_estimate/joint/pos

估计的关节位置

SensorDataPublisher

/joint_states
编码器原始数据

/imu
imu原始数据

/humanoid_mpc_observation
Type: ocs2_msgs/mpc_observation
观测值包含当前状态,mpc端接收
mpc_observation是简化手部自由度的，只用在mpc内部

/humanoid_controller/com/com_rf_diff_des
type：std_msgs/Float64MultiArray
右脚质心期望位置 xyz, 单位(m)

/humanoid_controller/optimizedInput_mrt/joint_vel
type：std_msgs/Float64MultiArray
从mpc(mrt)取得的关节期望速度

/humanoid_controller/optimized_mode
type：std_msgs/Float64
mpc 给出的 mode

/state_estimate/joint/vel

估计的关节速度

/state_estimate/base/pos_xyz

估计的躯干位置,顺序为xyz

/humanoid_VR_hand_control

/humanoid_wbc_observation
Type: ocs2_msgs/mpc_observation
观测值包含当前状态,mpc端接收
wbc_observation是全自由度,包含手部自由度

/humanoid_Arm_time_target_control